
#include "rviz_planner/costmap.hpp"
#include <iostream>

namespace rviz_planner
{

    Costmap::Costmap()
    {
        initTestData();
        std::cout << "Costmap::Costmap()" << std::endl;
    }
    Costmap::~Costmap()
    {
    }

    void Costmap::initTestData()
    {
        //home 场景
        // size_x_ = 240;
        // size_y_ = 201;
        // origin_x_ = -6.01;
        // origin_y_ = -5.04;
        
        // agv 场景
        size_x_ = 1449;
        size_y_ = 804;
        origin_x_ = -36.30;
        origin_y_ = -20.20;
        resolution_ = 0.05;
    }
    bool Costmap::worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my)
    {
        if (wx < origin_x_ || wy < origin_y_)
        {
            return false;
        }

        mx = static_cast<unsigned int>(std::round((wx - origin_x_) / resolution_));
        my = static_cast<unsigned int>(std::round((wy - origin_y_) / resolution_));

        if (mx < size_x_ && my < size_y_)
        {
            return true;
        }
        return false;
    }
    void Costmap::mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy)
    {
        wx = origin_x_ + mx * resolution_;
        wy = origin_y_ + my * resolution_;
    }

    unsigned int Costmap::getSizeInCellsX()
    {
        return size_x_;
    }

    unsigned int Costmap::getSizeInCellsY()
    {
        return size_y_;
    }
}